import os

from ament_index_python.packages import get_package_share_directory
import launch_ros.actions
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration, PythonExpression
from launch_ros.actions import LoadComposableNodes
from launch_ros.actions import Node
from launch_ros.descriptions import ComposableNode
from nav2_common.launch import RewrittenYaml

def generate_robot_node(robot_urdf,child):

    urdf_path = os.path.join(
        get_package_share_directory('robot_bringup'),
        'urdf',
        robot_urdf)

    with open(urdf_path, 'r') as infp:
        robot_desc = infp.read()

    return launch_ros.actions.Node(
            package='robot_state_publisher',
            executable='robot_state_publisher',
            name=f'robot_state_publisher_{child}',
            output='screen',
            parameters=[{
                'robot_description': robot_desc
            }],
    )
  
    # return launch_ros.actions.Node(
    #     package='robot_state_publisher',
    #     executable='robot_state_publisher',
    #     name=f'robot_state_publisher_{child}',
    #     arguments=[os.path.join(get_package_share_directory('mower_description'), 'urdf', robot_urdf)],
    # )

def generate_static_transform_publisher_node(translation, rotation, parent, child):
    # 静态发布节点，每次生成一个节点,static_transform_publisher 工具通常用于发布单个静态变换
    return launch_ros.actions.Node(
        package='tf2_ros',
        executable='static_transform_publisher',
        name=f'base_to_{child}',
        arguments=[translation[0], translation[1], translation[2], rotation[0], rotation[1], rotation[2], parent, child],
    )


def generate_launch_description():
    
    brushless_senior_diff = LaunchConfiguration('brushless_senior_diff', default='true')
    brushless_senior_diff_ = GroupAction(
        condition=IfCondition(brushless_senior_diff),
        actions=[
            generate_robot_node('brushless_senior_diff.urdf','brushless_senior_diff'), # 动态变换
            generate_static_transform_publisher_node(['0.272', '0', '0.257'], ['0', '0', '0'], 'base_footprint', 'laser'),      # 静态变换
            # generate_static_transform_publisher_node(['0.08', '0', '0.25'], ['0', '0', '0'], 'base_footprint', 'camera_link'),   

            # 将这些静态变换添加到urdf文件中，有些应属于静态变换，有些属于动态变换，目前这个有点问题-20250117-dw.liu
            generate_static_transform_publisher_node(['0', '0', '0'],['0', '0','0'],'base_footprint','base_link'),
            generate_static_transform_publisher_node(['0', '0', '0'],['0', '0','0'],'base_footprint','imu_link'),
            generate_static_transform_publisher_node(['0', '0', '0'],['0', '0','0'],'base_footprint','rtk_gps'),
            
    ])

    
    # 负责发布机器人关节数据信息，通过joint_states话题发布
    joint_state_publisher_node = launch_ros.actions.Node(
            package='joint_state_publisher', 
            executable='joint_state_publisher', 
            name='joint_state_publisher',
    )
    

    # Create the launch description and populate
    ld = LaunchDescription()

    # Set environment variables
    # Declare the launch options

    #ld.add_action(declare_use_composition_cmd)

    # Add the actions to launch all of the localiztion nodes

    ld.add_action(brushless_senior_diff_)
    ld.add_action(joint_state_publisher_node)

    return ld
